//
//  Frame.cpp
//  markerARLib
//
//  Created by CADCG on 4/15/16.
//  Copyright © 2016 CvidKal. All rights reserved.
//

#include "Frame.hpp"
#include "fast.hpp"

using namespace std;
using namespace cv;
namespace markerAR
{
    
Frame::Frame(const cv::Mat &img,const IntrinsicMatrix* K,const Pose &C)
{
    this->K  = K;
    this->C.clear();
    this->C.push_back(C);
    if(img.type() != CV_8U)
        cv::cvtColor(img, this->img, CV_BGR2GRAY);
    else
        this->img = img.clone();
    cv::GaussianBlur(this->img, this->img, cv::Size(5,5), 1);

};
Frame::~Frame(){
    for (int i=0; i<fts.size(); ++i) {
        delete fts[i];
    }
};

void Frame::detectFastCorners()
{
  CV_Assert(img.type()==CV_8U);
  int GridRows = Config::grid_rows;
  int GridCols = Config::grid_cols;
  int b = Config::fastThreshold;
  int rowStep = img.rows/GridRows;
  int colStep = img.cols/GridCols;
  
  int rowSize,colSize;
    vector<xy> corners;
  fts.reserve(500);
  int border = Config::patchSize;
    double fx = this->K->fx;
  
  for(int i=0;i<GridRows;++i)
  {
    int baseRow = rowStep*i;
    byte* rowBase = img.ptr<byte>(baseRow);
    
    if(i == GridRows)
      rowSize = img.rows-baseRow+1;
    else
      rowSize = rowStep;
    
    for(int j=0;j<GridCols;++j)
    {
      int baseCol = colStep*j;
      byte* data = rowBase + baseCol;
      if (j == GridCols)
        colSize =img.cols-colStep*j;
      else
        colSize = colStep;
        fast11_detect(data, rowSize, colSize, img.step1(),b,corners);
      for(int k=0;k<corners.size();++k)
      {
      //remove the point in the image border
        corners[k].x+=baseCol;
        corners[k].y+=baseRow;
        if (corners[k].x<border ||corners[k].x>img.cols-border||corners[k].y<border||corners[k].y>img.rows-border)
          continue;
          fts.push_back(new Feature(corners[k].x,corners[k].y,this));
      }
    }
  }
  
  
}

void Frame::extractPatch()
{
  short mean = static_cast<short>(cv::mean(img)[0]);
  for(int i=0;i<fts.size();++i)
  {
    uchar* p = img.ptr<uchar>((int)fts[i]->_pos.y())+(int)fts[i]->_pos.x();
    fts[i]->patch_with_border_ =  new Patch(p,img.cols);

  }
  
  
}

}